# -- coding: utf-8 --

import queue
import sys

sys.path.append(r'D:\code\git\ywlydd\qdzs\MVSCam')

import threading
import msvcrt
import numpy as np
from mvutils import *
from ctypes import *

import math
import cv2
import time
import inspect

# sys.path.append("../MvImport")
sys.path.append("./MVSCam")

from MvCameraControl_class import *

winfun_ctype = WINFUNCTYPE

stFrameInfo = POINTER(MV_FRAME_OUT_INFO_EX)
pData = POINTER(c_ubyte)
FrameInfoCallBack = winfun_ctype(None, pData, stFrameInfo, c_void_p)


def Async_raise(tid, exctype):
    tid = ctypes.c_long(tid)
    if not inspect.isclass(exctype):
        exctype = type(exctype)
    res = ctypes.pythonapi.PyThreadState_SetAsyncExc(
        tid, ctypes.py_object(exctype))
    if res == 0:
        raise ValueError("invalid thread id")
    elif res != 1:
        ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, None)
        raise SystemError("PyThreadState_SetAsyncExc failed")


def Stop_thread(thread):
    Async_raise(thread.ident, SystemExit)


def Work_thread(self):
    stOutFrame = MV_FRAME_OUT()
    while True:
        self.lock.acquire()
        ret = self.cam.MV_CC_GetImageBuffer(stOutFrame, 8000)
        # print('MV_CC_GetImageBuffer', ret)
        if 0 == ret:
            # buf_cache = (c_ubyte * stOutFrame.stFrameInfo.nFrameLen)()
            numArray = cam_cvtFrame(self.cam, stOutFrame.pBufAddr, stOutFrame.stFrameInfo)
            # 合并OpenCV到Tkinter界面中
            if self.frame_callback is not None and numArray is not None:
                self.frame_callback(numArray)
                # self.frame_callback([numArray,self.nConnectionNum])

        else:
            # print("no data, nret = ", (ret))
            pass
        nRet = self.cam.MV_CC_FreeImageBuffer(stOutFrame)
        # 释放锁
        self.lock.release()

    return 0


num_device = {}
deviceList = MV_CC_DEVICE_INFO_LIST()
# tlayerType = MV_GIGE_DEVICE | MV_USB_DEVICE
tlayerType = MV_GIGE_DEVICE


def GetCamNum():
    aa = len(num_device)
    if aa > 0:
        return aa
    # ch:枚举设备 | en:Enum device
    ret = MvCamera.MV_CC_EnumDevices(tlayerType, deviceList)
    if ret != 0:
        print("enum devices fail! ret[0x%x]" % ret)
        # sys.exit()

    if deviceList.nDeviceNum == 0:
        print("find no device!")
        ##sys.exit()

    print("Find %d devices!" % deviceList.nDeviceNum)

    for i in range(0, deviceList.nDeviceNum):
        mvcc_dev_info = cast(deviceList.pDeviceInfo[i], POINTER(
            MV_CC_DEVICE_INFO)).contents
        if mvcc_dev_info.nTLayerType == MV_GIGE_DEVICE:
            print("\ngige device: [%d]" % i)
            strModeName = ""
            for per in mvcc_dev_info.SpecialInfo.stGigEInfo.chModelName:
                strModeName = strModeName + chr(per)
            print("device model name: %s" % strModeName)

            nip1 = (
                    (mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0xff000000) >> 24)
            nip2 = (
                    (mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x00ff0000) >> 16)
            nip3 = (
                    (mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x0000ff00) >> 8)
            nip4 = (mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x000000ff)
            ip = "%d.%d.%d.%d" % (nip1, nip2, nip3, nip4)
            print("current ip: %d.%d.%d.%d\n" % (nip1, nip2, nip3, nip4))
            if ip not in num_device.keys():
                num_device[str(i)] = i
                num_device[ip] = i
                num_device[str(nip4)] = i
        elif mvcc_dev_info.nTLayerType == MV_USB_DEVICE:
            print("\nu3v device: [%d]" % i)
            strModeName = ""
            for per in mvcc_dev_info.SpecialInfo.stUsb3VInfo.chModelName:
                if per == 0:
                    break
                strModeName = strModeName + chr(per)
            print("device model name: %s" % strModeName)

            strSerialNumber = ""
            for per in mvcc_dev_info.SpecialInfo.stUsb3VInfo.chSerialNumber:
                if per == 0:
                    break
                strSerialNumber = strSerialNumber + chr(per)
            print("user serial number: %s" % strSerialNumber)

    return deviceList.nDeviceNum


GetCamNum()


class MVSCam:

    def __init__(self, ip=None, triggermode='off', frame_callback=None):
        self.b_thread_closed = False
        self.b_start_grabbing = False
        self.triggermode = triggermode
        self.frame_callback = frame_callback
        self.lock = threading.Lock()
        self.pData = None
        self.data_buf = None
        if ip is not None:
            self.open(ip, triggermode, frame_callback)

    def open(self, ip, triggermode='off', frame_callback=None):
        # nConnectionNum = input("please input the number of the device to connect:")
        # nConnectionNum = 0
        self.ip = ip
        print(f'MVSCam open {ip} {triggermode}')
        self.triggermode = triggermode
        self.frame_callback = frame_callback
        # print(num_device)
        nConnectionNum = ip
        ip = str(ip)
        if isinstance(ip, str) and str(ip) in num_device:
            nConnectionNum = int(num_device[str(ip)])
        else:
            return 0

        self.nConnectionNum = nConnectionNum
        # if self.b_start_grabbing:
        #     return 1

        if int(nConnectionNum) >= deviceList.nDeviceNum:
            print("intput error!")
            return 0

        # ch:创建相机实例 | en:Creat Camera Object
        cam = MvCamera()
        self.cam = cam
        # ch:选择设备并创建句柄 | en:Select device and create handle
        stDeviceList = cast(deviceList.pDeviceInfo[int(nConnectionNum)], POINTER(MV_CC_DEVICE_INFO)).contents

        ret = cam.MV_CC_CreateHandle(stDeviceList)
        if ret != 0:
            print("create handle fail! ret[0x%x]" % ret)
            return 0

        # ch:打开设备 | en:Open device
        ret = cam.MV_CC_OpenDevice(MV_ACCESS_Exclusive, 0)
        if ret != 0:
            print('xxxxxx')
            print("open device fail! ret[0x%x]" % ret)
            print(ip)
            return 0
            # sys.exit()

        # ch:探测网络最佳包大小(只对GigE相机有效) | en:Detection network optimal package size(It only works for the GigE camera)
        if stDeviceList.nTLayerType == MV_GIGE_DEVICE:
            nPacketSize = cam.MV_CC_GetOptimalPacketSize()
            if int(nPacketSize) > 0:
                ret = cam.MV_CC_SetIntValue("GevSCPSPacketSize", nPacketSize)
                if ret != 0:
                    print("Warning: Set Packet Size fail! ret[0x%x]" % ret)
            else:
                print("Warning: Get Packet Size fail! ret[0x%x]" % nPacketSize)

        if 0:
            ret = cam.MV_CC_SetEnumValue("PixelFormat", PixelType_Gvsp_BayerGB8)
            if ret != 0:
                print("error: Set PixelFormat fail [%x]" % ret)

        if self.triggermode == 'off':
            # ch:设置触发模式为off | en:Set trigger mode as off
            ret = cam.MV_CC_SetEnumValue("TriggerMode", MV_TRIGGER_MODE_OFF)
            if ret != 0:
                print("set trigger mode fail! ret[0x%x]" % ret)
                return 0
            if self.frame_callback is not None and 0:
                # ch:注册抓图回调 | en:Register image callback

                def mvs_image_callback(pData, pFrameInfo, pUser):
                    stFrameInfo = cast(pFrameInfo, POINTER(MV_FRAME_OUT_INFO_EX)).contents
                    if stFrameInfo:
                        print("get one frame: Width[%d], Height[%d], nFrameNum[%d]" % (
                        stFrameInfo.nWidth, stFrameInfo.nHeight, stFrameInfo.nFrameNum))
                        # self = cast(pUser, POINTER(MVSCam)).contents
                        numArray = cam_cvtFrame(self.cam, pData, stFrameInfo)
                        # 合并OpenCV到Tkinter界面中
                        if self.frame_callback is not None and numArray is not None:
                            self.frame_callback(numArray)
                            # self.frame_callback([numArray,self.nConnectionNum])

                CALL_BACK_FUN = FrameInfoCallBack(mvs_image_callback)

                ret = cam.MV_CC_RegisterImageCallBackEx(CALL_BACK_FUN, None)
                if ret != 0:
                    print("register image callback fail! ret[0x%x]" % ret)
                    return 0

        elif self.triggermode == 'continuous':
            ret = cam.MV_CC_SetEnumValue("AcquisitionMode", MV_ACQ_MODE_CONTINUOUS)
            if ret != 0:
                print("set trigger mode fail! ret[0x%x]" % ret)
                return 0
        else:
            # ch:设置触发模式为off | en:Set trigger mode as off
            ret = cam.MV_CC_SetEnumValue("TriggerMode", MV_TRIGGER_MODE_ON)
            if ret != 0:
                print("set trigger mode fail! ret[0x%x]" % ret)
                return 0

            if self.triggermode == 'soft':
                ret = self.cam.MV_CC_SetEnumValue(
                    "TriggerSource", MV_TRIGGER_SOURCE_SOFTWARE)
                if ret != 0:
                    print('set triggersource fail! ret = ', (ret))
            elif self.triggermode == 'hard':
                ret = cam.MV_CC_SetEnumValue(
                    "TriggerSource", MV_TRIGGER_SOURCE_LINE0)
                if ret != 0:
                    print('set triggersource fail! ret = ', (ret))
            else:
                pass

        # ch:获取数据包大小 | en:Get payload size
        stParam = MVCC_INTVALUE()
        memset(byref(stParam), 0, sizeof(MVCC_INTVALUE))

        ret = cam.MV_CC_GetIntValue("PayloadSize", stParam)
        if ret != 0:
            print("get payload size fail! ret[0x%x]" % ret)
            return 0
        nPayloadSize = stParam.nCurValue

        ret = cam.MV_CC_StartGrabbing()
        if ret != 0:
            print('show error', 'start grabbing fail! ret = ', (ret))
            return 0
        self.b_exit = False
        self.b_start_grabbing = True
        print("start grabbing successfully!")
        if self.triggermode in ['hard', 'off'] and self.frame_callback is not None:
            try:
                # 生成互斥锁对象
                self.lock = threading.Lock()

                # 获取锁，未获取到程序将会阻塞(当一个线程在执行被上锁的代码块时，将不允许切换到其他线程)
                # self.lock.acquire()

                # 释放锁
                # self.lock.release()

                # 判断该锁对象是否处于上锁状态
                # self.lock.locked()

                # self.n_win_gui_id = random.randint(1,10000)
                self.h_thread_handle = threading.Thread(
                    target=Work_thread, args=(self,), daemon=True)
                self.h_thread_handle.start()
                self.b_thread_closed = True
            except:
                print('show error', 'error: unable to start thread')
                False == self.b_start_grabbing

        data_buf = (c_ubyte * nPayloadSize)()

        self.pData = byref(data_buf)
        self.nDataSize = nPayloadSize
        stFrameInfo = MV_FRAME_OUT_INFO_EX()
        memset(byref(stFrameInfo), 0, sizeof(stFrameInfo))
        self.cam = cam
        self.stFrameInfo = stFrameInfo
        self.data_buf = data_buf
        return 1

    def setExposureTime(self, stFloatParam_exposureTime):
        ret = self.cam.MV_CC_SetFloatValue("ExposureTime", stFloatParam_exposureTime)
        return ret

    def getFrame1(self):
        cam = self.cam
        if self.pData is None:
            return None
        pData = self.pData
        nDataSize = self.nDataSize
        stFrameInfo = self.stFrameInfo
        ret = cam.MV_CC_GetOneFrameTimeout(pData, nDataSize, stFrameInfo, 3500)

        if ret == 0:
            # print ("get one frame: Width[%d], Height[%d], nFrameNum[%d]"  % (stFrameInfo.nWidth, stFrameInfo.nHeight, stFrameInfo.nFrameNum))
            pass
        else:
            print("no data[0x%x] ip:%s" % (ret, self.ip))
            return None

        npArray = cam_cvtFrame(cam, pData, stFrameInfo)
        if 0:
            cn = nDataSize / (stFrameInfo.nHeight * stFrameInfo.nWidth)
            cn = int(math.ceil(cn))
            npArray = np.zeros(
                (stFrameInfo.nHeight, stFrameInfo.nWidth, cn), dtype=np.uint8)
            npArrayPtr = npArray.ctypes.data_as(POINTER(c_ubyte))
            memmove(npArrayPtr, pData, nDataSize)
            if PixelType_Gvsp_YUV422_YUYV_Packed == stFrameInfo.enPixelType:
                # print(cv2.COLOR_YUV2BGR_YUYV)
                # print(cv2.COLOR_YUV2BGR_Y422)
                npArray = cv2.cvtColor(npArray, cv2.COLOR_YUV2BGR_YUYV)
            elif PixelType_Gvsp_BayerGB8 == stFrameInfo.enPixelType:
                npArray = cv2.cvtColor(npArray, cv2.COLOR_BAYER_GB2RGB)
            elif PixelType_Gvsp_BayerRG8 == stFrameInfo.enPixelType:
                npArray = cv2.cvtColor(npArray, cv2.COLOR_BAYER_RG2RGB)
            else:
                print('stFrameInfo.enPixelType', stFrameInfo.enPixelType)
                pass

        return npArray

    def getFrame(self):
        self.lock.acquire()
        im = self.getFrame1()
        self.lock.release()
        return im

    def stop(self):
        cam = self.cam
        if self.data_buf is None:
            return 0
        data_buf = self.data_buf

        try:
            if True == self.b_thread_closed:
                Stop_thread(self.h_thread_handle)
                self.b_thread_closed = False
            # msvcrt.getch()
            # ch:停止取流 | en:Stop grab image
            ret = cam.MV_CC_StopGrabbing()
            if ret != 0:
                print("stop grabbing fail! ret[0x%x]" % ret)
                del data_buf
                # sys.exit()

            # ch:关闭设备 | Close device
            ret = cam.MV_CC_CloseDevice()
            if ret != 0:
                print("close deivce fail! ret[0x%x]" % ret)
                del data_buf
                # sys.exit()

            # ch:销毁句柄 | Destroy handle
            ret = cam.MV_CC_DestroyHandle()
            if ret != 0:
                print("destroy handle fail! ret[0x%x]" % ret)
                del data_buf
                # sys.exit()

            if data_buf is not None:
                del data_buf

        except Exception as e:
            print(f'cam.stop err')

        return 0

    def close(self):
        self.stop()
        self.cam = None

    def isopen(self):
        return self.cam is not None


def test_MVSCam():
    cam = MVSCam()
    '169.254.183.134'
    cam.open('134')
    t0 = time.time()
    for i in range(10000):
        im = cam.getFrame()
        t1 = time.time()
        nframe = i + 1
        tt = (t1 - t0) / nframe
        # im = cv2.resize(im, (640, 480))
        # cv2.imshow('im', im)
        print(f'{i} {tt:.3f} {im.shape}')
        # cv2.waitKey(100)

    cam.stop()


def test_MVSCam1():
    ips = [201 + i for i in range(3)]
    cams = [MVSCam() for x in ips]
    [cams[i].open(ips[i]) for i in range(len(ips))]
    for i in range(100):
        ims = [cams[i].getFrame() for i in range(len(ips))]
        ims = [cv2.resize(im, (640, 480)) for im in ims]
        [cv2.imshow(f'im{i}', ims[i]) for i in range(len(ips))]
        print(i)
        cv2.waitKey(100)

    # cam.stop()
    return 0


def test_MVSCam_tri():
    cam = MVSCam()
    import queue
    q = queue.Queue(maxsize=1)

    def frame_callback(im):
        im = cv2.resize(im, (640, 480))
        q.put(im)

    cam.open('33', 'hard', frame_callback)
    im = np.zeros((480, 640, 3), dtype='uint8')
    for i in range(1000):
        try:
            if q.qsize() > 0:
                im = q.get(False, 0.1)
            else:
                time.sleep(1)
                continue
        except queue.Empty:
            pass
        cv2.imshow('im', im)
        cv2.waitKey(100)
        print('time out')

    cam.stop()


def test_MVSCam_tri_soft():
    cam = MVSCam()
    import queue
    q = queue.Queue()

    def frame_callback(im):
        im = cv2.resize(im, (640, 480))
        q.put(im)

    # cam.open(0, 'soft', frame_callback)
    cam.open('201', 'hard', frame_callback)
    im = np.zeros((480, 640, 3), dtype='uint8')
    for i in range(1000):
        t0 = time.time()
        im = q.get()
        t1 = time.time()
        tt = t1 - t0
        cv2.imshow('im', im)
        cv2.waitKey(100)
        print('time out')

    cam.stop()
    return 0


if __name__ == "__main__":
    # test_MVSCam_tri_soft()
    # test_MVSCam()
    test_MVSCam_tri()
